//
// Created by 12611 on 2023/6/28.
//
#include "math.h"
#include "utils.h"
#include "Dog.h"

float door_fun(float t)
{
    float num=0;
    if(t<=0.3)
        num=sin(PI * t / 0.6);
    else if(t>0.3&&t<0.5) num=1;
    else if(t>=0.5&&t<0.8){
        t-=0.5;
        num=cos(PI / 2 + PI * t / 0.6)+1;
    }
    else num=0;
    return num;
}

float line_fun(float t)
{
    float num=0;
    if(t <= 0.2) num=0;
    else if(t > 0.2 && t < 0.6 ){
        t -= 0.2;
        num = sin( PI * t / 0.8 );
    }else num = 1;
    return num;
}

//float move_fun(float t)
//{
//    float T=GAIT_T;
//    float num=0;
//    float Tw1=2,Tw2=3,Tw3=4;
//    if(t<=1){
//        num=0.03 * cos(PI * t);
//    }
//    else if(t>1&&t<Tw1)
//    {
//        t-=1;
//        num=0.02 * cos(PI + PI * t / 2);
//    }
//    else if(t>Tw1&&t<=Tw2){
//        num=0;
//    }
//    else if(t>Tw2&&t<Tw3){
//        t-=Tw2;
//        num=-0.05 + 0.05 * cos(PI * t / 2);
//    }
//    else{
//        t-=Tw3;
//        num=-0.03 + 0.03 * cos(PI + PI * t / 4);
//    }
//    return num;
//}

float move_fun(float t)//test
{
    float T=GAIT_T;
    float num=0;
    float Tw1=2,Tw2=3,Tw3=5;
    if(t<=Tw1){
        num=0.01 * cos(PI * t / Tw1) + 0.03;
    }
    else if(t>Tw1&&t<=Tw2){
        t-=Tw1;
        num=-0.08 * sin(PI * t / 2) + 0.02;
    }
    else if(t>Tw2&&t<Tw3){
        t-=Tw2;
        num=-0.06 + 0.04 * sin(PI + PI * t / 4);
    }
    else{
        t-=Tw3;
        num=-0.01 + 0.09 * cos(PI + PI * t / 2);
    }
    return num;
}

float move_fun_bal(float t)
{
    float num=0;
    float Tw=3;
    if(t<=2){
        num=0.03;
    }
    else if(t>2&&t<3)
    {
        t-=2;
        num=0.03 * cos(PI * t);
    }
    else if(t>=3 && t<=5){
        num=-0.03;
    }
    else if(t>5){
        t-=5;
        num=0.03 * cos(PI + PI * t);
    }
    return num;
}

float Upstair_fun_z(float t)
{
    float num=0;
    if(t<1){
        num = 0;
    }
    else if(t>=1&&t<=3){
        num = 0;
    }
    else if(t>3&&t<4){
        t-=3;
        num = -0.06 * sin(PI * t / 2);
    }
    else if(t>=4&&t<5){
        t-=4;
        num = -0.06 - 0.04 * sin(PI * t /2);
     }
    else if(t>=5){
        t-=5;
        num = -0.06 + 0.04 * cos(PI + PI * t / 2);
    }
    return num;
}

float Upstair_fun_y(float t,int i)
{
    float H = 1.5 * GAIT_H;
    float num = 0;
    if(t <= 1){
        if(i < 2)
            num = H * cos(PI * t / 2);
    }
    else if(t > 1 && t <= 3){
        if(i < 2)
            num = 0;
    }
    else if(t > 3 && t <= 4) {
        t -= 3;
        if (i < 2)
            num = H * sin(PI * t / 2);
    }
    else if(t > 4){
        if(i < 2)
            num = H;
    }
    return num;
}

float Downstair_fun_z(float t)
{
    float num=0;
    if(t<1){
        num = 0.05 * sin(PI * t / 2);
    }
    else if(t>=1&&t<=3){
        num = 0.05;
    }
    else if(t>3&&t<4){
        t-=3;
        num = 0.03 - 0.07 * sin(PI * t / 2) ;
    }else{
        num = -0.04;
    }
    return num;
}

float Downstair_fun_y(float t,int i)
{
    float H=GAIT_H;
    float num=2 * H;
    if(t<1)
    {
        num = 2 * H * cos(PI * t / 2 + PI ) + 2 * H;
    }
    else if(t>3&&t<4)
    {
        t-=3;
        if(i<2)
            num += 2 * H * sin(PI * t / 2);
    }
    else if(t>=4&&t<5)
    {
        if(i<2)
            num += 2 * H;
    }
    else if(t>=5)
    {
        t-=4;
        if(i<2)
            num += 2 * H * sin(PI * t / 2);
    }
    return num;
}

float Downstair_door_fun(float t){
    float num=0;
    if(t<=0.3)
        num=sin(PI * t / 0.6);
    else if(t>0.3&&t<0.5) num=1;
    else if(t>=0.5){
        t-=0.5;
        num= 3 * sin(PI / 2 + PI * t) - 2;
    }
    else num=0;
    return num;
}